#include <buola/buola.h>
#include <buola/app/ccmdline.h>
#include <buola/gui/ctoolkit.h>
#include <buola/image/format.h>
#include <buola/image/io.h>
#include <buola/image/algorithm/calibration.h>
#include <buola/cv/ccloud.h>
#include <buola/scene/geodes/cpointcloud.h>
#include <buola/scene/cscene.h>
#include <buola/scene/cperspectivecamera.h>
#include <buola/scene/csceneview.h>
#include <buola/scene/controllers/ccameracontroller.h>
#include <buola/image/cgc.h>
#include <buola/mat/cmatfile.h>
#include <matio.h>

using namespace buola;

int main(int pNArg,char* pArgs[])
{
    buola_init(pNArg,pArgs);

    if(cmd_line().ArgCount()<2)
    {
        msg_info() << "please specify filename and image indices\n";
        return 1;
    }

    try
    {
        gui::get_toolkit()->GetOpenGLGC()->MakeCurrent();

        img::CCameraParameters lParams;
        lParams.SetMatrix(CPoint_d(525,525),CPoint_d(320,240));
        
        img::CImage_rgb8 lImage1;
        load(lImage1,io::arg(0)/("image_"+cmd_line().Arg(1)+".png"));

        mat::CMatFile lMatFile1(io::arg(0)/("kinect/frame_"+cmd_line().Arg(1)+".mat"));
        img::CImage_gray32f lDispImage1=lMatFile1.GetImage("disparity");
        for(int i=0;i<lDispImage1.Height();i++)
            for(int j=0;j<lDispImage1.Width();j++)
                lDispImage1(j,i)=-525.0f*0.08f/lDispImage1(j,i);
        
        cvi::CCloud_rgb8 lCloud1=cvi::make_cloud(lDispImage1,lImage1,lParams);
        
        img::CImage_rgb8 lImage2;
        load(lImage2,io::arg(0)/("image_"+cmd_line().Arg(2)+".png"));

        mat::CMatFile lMatFile2(io::arg(0)/("kinect/frame_"+cmd_line().Arg(2)+".mat"));
        img::CImage_gray32f lDispImage2=lMatFile2.GetImage("disparity");
        for(int i=0;i<lDispImage2.Height();i++)
            for(int j=0;j<lDispImage2.Width();j++)
                lDispImage2(j,i)=-525.0f*0.08f/lDispImage2(j,i);
        
        cvi::CCloud_rgb8 lCloud2=cvi::make_cloud(lDispImage2,lImage2,lParams);
        
        //create the scene
        scene::PScene lScene=new scene::CScene;
        
        scene::PPointCloud lPCloud1=new scene::CPointCloud(std::move(lCloud1));
        lScene->World()->AddObject(lPCloud1);
        scene::PPointCloud lPCloud2=new scene::CPointCloud(std::move(lCloud2));
        lScene->World()->AddObject(lPCloud2);

        scene::PPerspectiveCamera lCamera=new scene::CPerspectiveCamera;
        lCamera->SetFOV(2*atan2(240.0,525.0));
        lCamera->SetClipping(0.1,50.0);
        lCamera->LookAt({0,0,1},{0,0,0},{0,-1,0});

        scene::PSceneView lView=new scene::CSceneView;
        lView->SetCamera(lCamera);
        lView->AddScene(lScene);
        lView->AddController(new scene::CCameraController(lCamera));
        lView->Map(CSize_d(800,600));

        buola_main_loop();
    }
    catch(std::exception &pE)
    {
        msg_info() << "caught exception " << pE.what() << "\n";
    }

    return buola_finish();
}
